#include "servo.h"
#include <string>
#include <string.h>
#include "config.h"
#include "zuc/nml_intf/zucpos.h"
#include "errcode.h"
#include "jkutil/configparser.h"
#include "rtbus/rtbus.h"
#include "log/zuclog.h"
#include "axis/planner.h"
#include "jkutil/bitdataop.h"

#define AXIS_DEBUG(fmt, ...) zuclog_debug("SERVO", fmt, ##__VA_ARGS__)
#define AXIS_INFO(fmt, ...) zuclog_info("SERVO", fmt, ##__VA_ARGS__)

using namespace axis;
using namespace std;
using axis::MAX_SLAVE_SERVO_CNT;

enum StateWord
{
    NO_READY_SWON = 0x00,
    SWON_DIS,
    READY_SWON,
    SWON,
    OP_EN,
    QUICK_STOP_ACTIVE,
    FAULT_REACTION_ACTIVE,
    FAULT
};

static StateWord get_stateword(uint16_t sword)
{
    if ((sword & 0x004f) == 0x00)
    {
        return NO_READY_SWON;
    }
    else if ((sword & 0x004f) == 0x40)
    {
        return SWON_DIS;
    }
    else if ((sword & 0x006f) == 0x21)
    {
        return READY_SWON;
    }
    else if ((sword & 0x006f) == 0x23)
    {
        return SWON;
    }
    else if ((sword & 0x006f) == 0x27)
    {
        return OP_EN;
    }
    else if ((sword & 0x006f) == 0x07)
    {
        return QUICK_STOP_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x0f)
    {
        return FAULT_REACTION_ACTIVE;
    }
    else if ((sword & 0x004f) == 0x08)
    {
        return FAULT;
    }
    return NO_READY_SWON;
}

static const char* get_stateword_name(uint16_t sword)
{
    switch (get_stateword(sword))
    {
    case NO_READY_SWON:
        return "no_ready_swon";
    case SWON_DIS:
        return "swon_dis";
    case READY_SWON:
        return "ready_swon";
    case SWON:
        return "swon";
    case OP_EN:
        return "op_en";
    case QUICK_STOP_ACTIVE:
        return "quick_stop_active";
    case FAULT_REACTION_ACTIVE:
        return "fault_reaction_active";
    case FAULT:
        return "fault";
    default:
        return "unknown";
    }
}

static Servo _servo[MAX_SLAVE_SERVO_CNT];

const int SERVO_COMMU_WATCHDOG_DEFAULT_CNT = 50;

const uint8_t LED_BLUE_BREATH = 0x01;
const uint8_t LED_GREEN_BREATH = 0x02;
const uint8_t LED_YELLOW_BREATH = 0x03;
const uint8_t LED_RED_BREATH = 0x04;
const uint8_t LED_BLUE_Flicker = 0x08;
const uint8_t LED_GREEN_Flicker = 0x06;
const uint8_t LED_YELLOW_Flicker = 0x05;
const uint8_t LED_RED_Flicker = 0x07;

template <typename T> static T get_data(uint8_t* data) { return data[6] + (data[5] << 8) + (data[4] << 16) + (data[3] << 24); }

Servo::Servo()
{
    memset(&config_, 0, sizeof(config_));
    memset(&usr_cmd_, 0, sizeof(usr_cmd_));
    memset(&stat_, 0, sizeof(stat_));
    alive_flag_ = false;
    alive_watchdog_ = 0;

    memset(&cmd_, 0, sizeof(cmd_));
    memset(&cmd_servo_, 0, sizeof(cmd_servo_));
    memset(&feedback_, 0, sizeof(feedback_));
    memset(&feedback_servo_, 0, sizeof(feedback_servo_));
}
Servo::~Servo() {}
void Servo::init(int id)
{
    config_.slave_id = id;
    alive_flag_ = false;
    alive_watchdog_ = 0;
    reset_data();
    Util::ConfigParser p(ZUC_DEFAULT_ROOT_DIR "/configs/default/rtdev.xml");
    auto servopath = Util::ConfigParser::str_append_index("servo", get_id());
    p.get(servopath + ".actived", config_.actived, true);
    p.get(servopath + ".devid", config_.bus_devid_, true);
    p.get(servopath + ".bus", config_.bus_id_, true);
    p.get(servopath + ".sim", config_.sim_flag, true);
}
void Servo::reset_data()
{
    memset(&stat_, 0, sizeof(stat_));
    memset(&usr_cmd_, 0, sizeof(usr_cmd_));
}
void Servo::boot_init(int busid, int devid, const bool* sim)
{
    reset_data();
    if (busid >= 0)
    {
        config_.bus_id_ = busid;
    }
    if (devid >= 0)
    {
        config_.bus_devid_ = devid;
    }
    if (sim == nullptr)
    {
        config_.sim_flag = *sim;
    }
    if (!is_sim())
    {
        int index = get_id() + 1;
        master_->config_rxpdo(0x00C00000 + 0x20 * index, 8);    //ota feedback
        master_->config_rxpdo(0x00A00000 + 0x20 * index, 8);    //feedback
        master_->config_rxpdo(0x003FFFE0, 8);                   //error
        master_->config_txpdo(0x00600000 + 0x2000 * index, 8);  //cmd
    }
}
void Servo::shutdown()
{
    reset_data();
    // rtbus::BusMgr::get()->unregister_rx_handle(0x00C00000 + 0x20 * (get_id() + 1));
    // rtbus::BusMgr::get()->unregister_rx_handle(0x00A00000 + 0x20 * (get_id() + 1));
    // rtbus::BusMgr::get()->unregister_tx_handle(bus_id_, 0x00600000 + 0x2000 * (get_id() + 1));
    // bus_id_ = -1;
}
void Servo::comm_ecat_map()
{
    // auto bus = rtbus::BusMgr::get();
    // bus->register_tx_handle(0x01000000 * (get_id() + 1) + 0x604000, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint16_t cword;
    //     comm_write_control_word(cword);
    //     *(uint16_t*)data = cword;
    //     return 0;
    // });
    // bus->register_tx_handle(0x01000000 * (get_id() + 1) + 0x607A00, [this](uint32_t id, uint8_t* data, size_t size) {
    //     int32_t pos;
    //     comm_write_target_pos(pos);
    //     *(int32_t*)data = pos;
    //     return 0;
    // });
    // bus->register_tx_handle(0x01000000 * (get_id() + 1) + 0x60b200, [this](uint32_t id, uint8_t* data, size_t size) {
    //     int16_t torque;
    //     comm_write_target_torque(torque);
    //     *(int16_t*)data = torque;
    //     return 0;
    // });
    // bus->register_tx_handle(0x01000000 * (get_id() + 1) + 0x606000, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint8_t opmode;
    //     comm_write_opmode(opmode);
    //     *(uint8_t*)data = opmode;
    //     return 0;
    // });
    // bus->register_tx_handle(0x01000000 * (get_id() + 1) + 0x700000, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint8_t led;  //only for max
    //     comm_write_led(led);
    //     *(uint8_t*)data = led;
    //     return 0;
    // });

    // bus->register_rx_handle(0x01000000 * (get_id() + 1) + 0x604100, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint16_t sword = *(uint16_t*)data;
    //     comm_read_status_word(sword, false);
    //     return 0;
    // });
    // bus->register_rx_handle(0x01000000 * (get_id() + 1) + 0x606400, [this](uint32_t id, uint8_t* data, size_t size) {
    //     int32_t pos = *(int32_t*)data;
    //     comm_read_actpos(pos);
    //     return 0;
    // });
    // bus->register_rx_handle(0x01000000 * (get_id() + 1) + 0x607700, [this](uint32_t id, uint8_t* data, size_t size) {
    //     int16_t torque = *(int16_t*)data;
    //     comm_read_acttorq(torque);
    //     return 0;
    // });
    // bus->register_rx_handle(0x01000000 * (get_id() + 1) + 0x606100, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint8_t opmode = *(uint8_t*)data;
    //     comm_read_opdis(opmode);
    //     return 0;
    // });
}

void Servo::comm_jkcan_tx_map(int busid)
{
    int index = get_id() + 1;

    // rtbus::BusMgr::get()->register_tx_handle(busid, 0x00600000 + 0x2000 * index, [this](uint32_t id, uint8_t* data, size_t size) {
    //     uint16_t cword = this->commu.tx().ctrlword;

    //     comm_write_control_word(cword);
    //     data[0] = cword >> 8 & 0xff;
    //     data[1] = cword & 0xff;
    //     uint8_t op = 0;
    //     comm_write_opmode(op);
    //     data[2] = op & 0x0f;
    //     data[2] |= (0 & 0x07) << 4;    //(s->get_collision_level() & 0x07) << 4;
    //     data[2] |= ((1) & 0x01) << 7;  //((!s->is_dyn_forward()) & 0x01) << 7;
    //     int32_t pos;
    //     comm_write_target_pos(pos);
    //     if (pos < 0)
    //     {
    //         data[3] = (abs(pos) >> 16) | 0x80;
    //     }
    //     else
    //     {
    //         data[3] = (abs(pos) >> 16) & 0xff;
    //     }

    //     data[4] = (abs(pos) >> 8) & 0xff;
    //     data[5] = abs(pos) & 0xff;

    //     int16_t torq;
    //     comm_write_target_torque(torq);
    //     if (torq < 0)
    //     {
    //         data[6] = (abs(torq) >> 8) | 0x80;
    //     }
    //     else
    //     {
    //         data[6] = (abs(torq) >> 8) & 0xff;
    //     }
    //     data[7] = abs(torq) & 0xff;
    //     return 8;
    // });
    // set_master(busid);
}
void Servo::comm_jkcan_map()
{
    int index = get_id() + 1;
    master_->config_rxpdo(0x00C00000 + 0x20 * index, 8);    //ota feedback
    master_->config_rxpdo(0x00A00000 + 0x20 * index, 8);    //feedback
    master_->config_rxpdo(0x003FFFE0, 8);                   //error
    master_->config_txpdo(0x00600000 + 0x2000 * index, 8);  //cmd

    // master_->register_sdo_handle()
    // rtbus::BusMgr::get()->register_rx_handle(0x00C00000 + 0x20 * index, [this](uint32_t id, uint8_t* Data, size_t size) {
    //     this->commu.set_rx().ota_pos_diff = Data[1] + (Data[0] << 8);
    //     this->commu.set_rx().ota_dual_encoder_diff = Data[3] + (Data[2] << 8);
    //     this->commu.set_rx().ota_dc_vol = Data[5] + (Data[4] << 8);
    //     return 8;
    // });
    // rtbus::BusMgr::get()->register_rx_handle(0x00A00000 + 0x20 * index, [this](uint32_t busid, uint8_t* data, size_t size) {
    //     if (is_sim())
    //     {
    //         return 8;
    //     }
    //     this->keep_alive();
    //     //若是第一次接收，则注册新的tx回调
    //     if ((int)busid != get_master_id() && get_master_id() == -1)
    //     {
    //         comm_jkcan_tx_map(busid);
    //     }

    //     this->commu.set_rx().statusword = (uint16_t)(data[1] + (data[0] << 8));
    //     this->commu.set_rx().opmode_display = data[2] & 0x0f;
    //     this->commu.set_rx().collision_lv = data[2] >> 4 & 0x07;
    //     this->commu.set_rx().backdrive_stat = data[2] >> 8 & 0x01;
    //     this->commu.set_rx().actual_pos = (data[5] + (data[4] << 8) + ((data[3] & 0x7f) << 16)) * ((data[3] & 0x80) ? -1 : 1);
    //     this->commu.set_rx().actual_torq = (data[7] + ((data[6] & 0x7f) << 8)) * ((data[6] & 0x80) ? -1 : 1);
    //     this->commu.set_rx().has_ext_enable_bit = true;

    //     return 8;
    // });
    // rtbus::BusMgr::get()->register_rx_handle(0x003FFFE0, [this](uint32_t id, uint8_t* data, size_t size) {
    //     if (this->get_id() != data[0] - 1)
    //         return -1;
    //     if (is_sim())
    //         return 0;
    //     this->commu.push_error(data[1] + (data[2] << 8), data[3]);
    //     return 8;
    // });
}
uint32_t Servo::get_ecode(uint8_t* category) const
{
    if (is_error())
    {
        if (category)
            *category = commu.rx_data_.error.category;
        return commu.rx_data_.error.ecode;
    }
    else
    {
        if (category)
            *category = 0;
        return 0;
    }
}
int Servo::config_offset_dir(int32_t offset, bool is_positive)
{
    if (usr_cmd_.enable)  //FIXME：应该还有别的状态
        return -1;
    config_.zero_offset_ = offset;
    config_.dir_ = is_positive ? 1 : -1;
    AXIS_DEBUG("joint %d: offset : %d dir : %d\n", get_id(), config_.zero_offset_, config_.dir_);
    return 0;
}

int Servo::set_cmd_pos(int pos)
{
    cmd_.pos = pos;
    cmd_servo_.pos = pos * get_dir() + get_zero_offset();
    return 0;
}

int Servo::set_cmd_torque(int torque, int grv_comp)
{
    if (usr_cmd_.opmode == OPMODE_CST)
    {
        cmd_.torque = torque;
        cmd_servo_.torque = torque * get_dir();
    }
    cmd_.grvcomp = grv_comp;
    cmd_servo_.grvcomp = grv_comp * get_dir();
    return 0;
}

int Servo::comm_write_control_word(uint16_t& cword)
{
    //控制字
    cword = usr_cmd_.control_word_;
    return 0;
}
int Servo::comm_write_led(uint8_t& led)
{
    if (is_error())
    {
        led = LED_RED_Flicker;
    }
    else if (is_enabled())
    {
        led = LED_GREEN_BREATH;
    }
    else
    {
        led = LED_BLUE_BREATH;
    }
    return 0;
}
int Servo::comm_write_target_pos(int32_t& pos)
{
    pos = cmd_servo_.pos;
    return 0;
}
int Servo::comm_write_target_torque(int16_t& torque)
{
    torque = cmd_servo_.grvcomp;
    return 0;
}
int Servo::comm_write_opmode(uint8_t& opmode)
{
    if (is_sim())
    {
        return 0;
    }
    opmode = usr_cmd_.opmode;
    return 0;
}

uint16_t Servo::handle_control_word(uint16_t sword, bool has_ext_enable_bit)
{
    uint16_t cword = 0;
    uint8_t* cword_ptr = (uint8_t*)&cword;

    auto cur_state = get_stateword(sword);
    bool iserror = false;
    bool stat_enable = false;
    switch (cur_state)
    {
    case NO_READY_SWON:
        cword = 0x00;
        break;
    case SWON_DIS:
        cword = 0x06;  //shutdown
        break;
    case READY_SWON:
        cword = usr_cmd_.enable ? 0x07 : 0x06;
        break;
    case SWON:
        cword = usr_cmd_.enable ? 0x0f : 0x06;
        break;
    case OP_EN: {
        stat_enable = true;
        cword = usr_cmd_.enable ? 0x0f : 0x06;
        //在支持扩展使能位的情况下，需要额外判定
        if (has_ext_enable_bit && (sword >> 14 & 0x01) == 0)
        {
            stat_enable = false;
        }

        break;
    }
    case QUICK_STOP_ACTIVE:
        cword = 0x06;  //shutdown
        break;
    case FAULT_REACTION_ACTIVE:
        iserror = true;
        break;

    case FAULT: {
        iserror = true;
        cword = usr_cmd_.control_word_;
        if (usr_cmd_.error_reset)
        {
            Util::set_bit(cword_ptr, 7);
            usr_cmd_.error_reset = false;
        }
        else
        {
            Util::clr_bit(cword_ptr, 7);
        }
        break;
    }
    default:
        break;
    }
    stat_.error_sta = iserror ? 1 : 0;

    //碰撞状态处理
    stat_.collision = sword >> 15 & 0x01;
    if (stat_.collision)
    {
        if (usr_cmd_.collision_reset > 0)
        {
            Util::set_bit(cword_ptr, 11);  //cword |= (1 << 11);
            usr_cmd_.collision_reset--;
        }
        else
        {
            Util::clr_bit(cword_ptr, 11);
        }
    }

    if (stat_enable != stat_.enable)
    {
        AXIS_DEBUG("servo %d enable status change %d->%d", get_id(), stat_.enable, stat_enable);
        stat_.enable = stat_enable;
    }

    //回零状态处理
    if (get_cur_opmode() == OPMODE_HOME)
    {
        if (usr_cmd_.home)
        {
            cword |= 0x10;
            usr_cmd_.home = 0;
        }
        if ((sword & 0x3400) == 0x1400)  //home reached
        {
            Util::clr_bit(cword_ptr, 12);
            stat_.home_sta = 1;
        }
    }
    return cword;
}

int Servo::get_sdo(uint16_t obj_id, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    if (is_sim())
        return 0;
    if (!master_)
        return -1;
    uint16_t id = (obj_id / 100 << 8) | (obj_id % 100 & 0xff);

    return master_->get_sdo(get_id() + 1, id, cb, timeout, retry);
}
int Servo::set_sdo(uint16_t obj_id, int* val, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    if (is_sim())
        return 0;
    if (!master_)
        return -1;
    uint16_t id = (obj_id / 100 << 8) | (obj_id % 100 & 0xff);

    return master_->set_sdo(get_id() + 1, id, (uint8_t*)val, 4, cb, timeout, retry);
}
void Servo::update()
{
    if (!is_sim())
    {
        //通讯获取到的状态数据处理
        if (commu.rx_data_.statusword != stat_.status_word)
        {
            AXIS_DEBUG("servo %d status %s(0x%x)->%s(0x%x)",
                       get_id(),
                       get_stateword_name(stat_.status_word),
                       stat_.status_word,
                       get_stateword_name(commu.rx_data_.statusword),
                       commu.rx_data_.statusword);
            stat_.status_word = commu.rx_data_.statusword;
        }
        feedback_servo_.pos = commu.rx_data_.actual_pos;
        feedback_.pos = (feedback_servo_.pos - get_zero_offset()) * get_dir();  //FIXME:此处的-号后面需要改
        if (!usr_cmd_.enable)
        {
            cmd_servo_.pos = feedback_servo_.pos;
        }

        feedback_servo_.torque = commu.rx_data_.actual_torq;
        feedback_.torque = feedback_servo_.torque * get_dir();
        stat_.act_opmode = commu.rx_data_.opmode_display;

        //通讯指令数据处理
        auto tmp_cword = handle_control_word(stat_.status_word, commu.rx_data_.has_ext_enable_bit);
        if (usr_cmd_.control_word_ != tmp_cword)
        {
            AXIS_DEBUG("servo %d cword 0x%x->0x%x", get_id(), usr_cmd_.control_word_, tmp_cword);
        }
        usr_cmd_.control_word_ = tmp_cword;
    }
    else
    {
        stat_.enable = usr_cmd_.enable;
        feedback_.pos = cmd_.pos;
        feedback_.torque = cmd_.torque;
        stat_.act_opmode = usr_cmd_.opmode;
        stat_.backdrive_flg = 0;
        stat_.tor_over_flg = 0;
    }
    check_alive();
}
void Servo::check_alive()
{
    if (!is_active())
    {
        alive_flag_ = false;
        return;
    }
    bool alive = false;
    if (is_sim())
    {
        alive = true;
    }
    else
    {
        if (alive_watchdog_ <= 0)
        {
            alive = false;
        }
        else
        {
            alive_watchdog_--;
            alive = true;
        }
    }
    if (alive != alive_flag_)
    {
        if (alive)
        {
            AXIS_DEBUG("Servo %d is online, sim=%d", get_id(), is_sim());
        }
        else
        {
            AXIS_DEBUG("Servo %d is offline,sim=%d", get_id(), is_sim());
            shutdown();
        }
        alive_flag_ = alive;
    }
}

void Servo::keep_alive() { alive_watchdog_ = SERVO_COMMU_WATCHDOG_DEFAULT_CNT; }

// ServoStatus servoRefreshStatus(uint8_t joint)
// {
//     ServoStatus ret;
//     memset(&ret, 0, sizeof(ret));
//     ret.enableState = _servo[joint].is_enabled();
//     ret.errorState = _servo[joint].is_error();
//     ret.errorCode = _servo[joint].get_ecode(&ret.servoErrCategory);
//     ret.actualOpMode = _servo[joint].get_cur_opmode();
//     ret.backDriveState = _servo[joint].is_backdriving();
//     ret.torOver = _servo[joint].is_torque_over();
//     ret.actualPosition = _servo[joint].get_cur_pos();
//     ret.actualTorque = _servo[joint].get_cur_torque();
//     ret.collisionState = _servo[joint].is_collision() || servo_status[joint].collisionState;
//     ret.connectState = _servo[joint].is_alive();
//     ret.homeState = _servo[joint].is_homing_done();

//     ret.servoVersion = _servo[joint].version_;
//     ret.collision_base = _servo[joint].collision_base_threshold_;
//     ret.collision_level = _servo[joint].collision_torque_threshold_;
//     ret.jointMaxLimit = _servo[joint].joint_max_limit_;
//     ret.jointMinLimit = _servo[joint].joint_min_limit_;
//     ret.firstPoweronDate = _servo[joint].first_poweron_date_;
//     ret.motorSupplier = _servo[joint].supplier_id_;
//     ret.instVoltage = _servo[joint].voltage_;
//     ret.instTemperature = _servo[joint].temperature_;
//     ret.insAveragePower = _servo[joint].average_power_;
//     ret.currentFluctuation = _servo[joint].current_fluctuation_;
//     ret.sumRunningCircles = _servo[joint].total_run_laps_;
//     ret.sumRunningTime = _servo[joint].total_run_time_;
//     ret.curRunningCircles = _servo[joint].current_run_laps_;
//     ret.curRunningTime = _servo[joint].current_run_time_;

//     return ret;
// }

Servo* axis::get_servo(uint16_t id)
{
    if (id >= MAX_SLAVE_SERVO_CNT)
    {
        return nullptr;
    }
    return &_servo[id];
}

int Servo::set_position(int pos, bool is_jaka)
{
    if (usr_cmd_.enable)
    {
        AXIS_DEBUG("Can't set position %d when servo%d is enabled", pos, get_id());
        return -1;
    }
    if (pos != (int)0xffffffff)
    {
        AXIS_DEBUG("Cannot set servo %d to position %d.", get_id(), pos);
        return -2;
    }
    if (is_jaka)
    {
        auto ret = set_sdo(SDO_INDEX_SERVO_HOME_VAL, &pos, [](uint8_t* rx_data, int s) -> int {
            if (s != 7)
                return s;
            return 0;
        });
        return ret;
    }
    return -3;
}
